#include <ltypes.h>
#include <lmacros.h>
#include <lvectors.h>
#include <ltriMesh.h>

#include "BVNode.h"

#include <vector>
#include <queue>

#include <boost/assert.hpp>

LLIB_BEGIN

template <typename BVNode_t>
geom::BVTree_CBT<BVNode_t>::~BVTree_CBT()
{
    delete[] _nodeList;
    delete[] _leafIndexList;
}

template <typename BVNode_t>
void geom::BVTree_CBT<BVNode_t>::Init( TriMesh& triMesh )
{
    _mesh = &triMesh;
    /* collect data for tree building up */
    std::vector<Vec3f> triCenter;
    Int32 faceNum = triMesh.GetFaceNum();
    Vec3f center;
    MeshFace face;
    FOR(F,0,faceNum) {
        face = triMesh.GetFace(F);
        center = ( face.GetVertex(0).GetRef() +
            face.GetVertex(1).GetRef() + 
            face.GetVertex(2).GetRef() ) / 3;
        triCenter.push_back(center);
    }
    /* build */
    build(triCenter);
}

// We build a complete binary tree as BVH
// thus tree can be arranged in array
template <typename BVNode_t>
void geom::BVTree_CBT<BVNode_t>::build( const std::vector<Vec3f>& elementPostions )
{
    BOOST_ASSERT(SZ(elementPostions) > 0);

    // remove old data
    delete[] _nodeList;
    delete[] _leafIndexList;

    /* index pointer to elementPostions */
    std::vector<Int32> leafIndex(SZ(elementPostions));
    FOR(I,0,SZ(leafIndex)) leafIndex[I] = I;
    /* arrange elements by index pointer */
    leafArrangement(elementPostions, leafIndex);

    /* the node numbers */
    _leafNum = SZ(elementPostions);
    _nodeNum = _leafNum * 2 - 1;
    /* allocate */
    _nodeList = new BVNode_t[_nodeNum];
    FOR(I,0,_nodeNum) _nodeList[I]._tree = this;
    _leafIndexList = new Int32[_leafNum];
    /* first and last node index */
    Int32 power = 0, tmp = _leafNum - 1;
    while(tmp > 0) {
        tmp >>= 1;
        ++power;
    }
    Int32 maxLeafNum = 1 << power;
    _firstLeafIndex = (1 << power) - 1;
    Int32 minLeafIndex = _nodeNum >> 1; // the minimum index leaf is in middle of array
    if(minLeafIndex == _firstLeafIndex) ++power;
    _lastLeafIndex = (1 << power) - 2;
    /* place the leaf element into _leafIndexList */
    Int32 curIndex = _leafNum * 2 - maxLeafNum;
    if(_leafNum == maxLeafNum) curIndex = 0;
    FOR(I,0,_leafNum) {
        _leafIndexList[I] = leafIndex[curIndex++];
        if(curIndex >= _leafNum)
            curIndex = 0;
    }
}

/* Arrange element by iterative division on longest axis,
 * result are saved in the order index list leafIndex,
 * in the form of leaf nodes of the corresponding complete binary tree.
 */
template <typename BVNode_t>
void geom::BVTree_CBT<BVNode_t>::leafArrangement( const std::vector<Vec3f>& elementPostions, std::vector<Int32>& leafIndex ) const
{
#define ELEMENT_POSITION(id)    elementPostions[ leafIndex[id] ]

    typedef std::pair<Int32,Int32> Range;
    std::queue<Range> callHeap;  // here we're using the nonrecursive mothed
    Range r(0,SZ(elementPostions)), r2;
    callHeap.push(r);
    Int32 rangeLen = 0;
    Int8  maxAxis = 0;
    Vec3f boundMax, boundMin;
    const static Int8 axisSelect[8] = { 2, 2, -1, 0, 1, -1, 1, 0 };
    Int32 tmp, division, pivotId, pivotVal;
    Float divideVal;
    Int32 lid, rid;

    while(!callHeap.empty()) {
        r = callHeap.front();
        callHeap.pop();
        rangeLen = r.second - r.first;
        if(rangeLen < 3) {
            if(rangeLen == 2) {
                if(ELEMENT_POSITION(r.first) > ELEMENT_POSITION(r.first+1))
                    std::swap(leafIndex[r.first],leafIndex[r.first+1]);
            }
            continue; // no need to go deep
        }

        // max bound axis selection
        boundMax = boundMin = ELEMENT_POSITION(r.first);
        FOR(pid,r.first+1,r.second) {
            const Vec3f& pos = ELEMENT_POSITION(pid);
            FOR(axis,0,3) {
                boundMin[axis] = min(boundMin[axis], pos[axis]);
                boundMax[axis] = max(boundMax[axis], pos[axis]);
            }
        }
        boundMax -= boundMin; // boundMax now saves boundRange
        tmp = 0;
        if(boundMax.x > boundMax.y) tmp |= 1;
        if(boundMax.x > boundMax.z) tmp |= 2;
        if(boundMax.y > boundMax.z) tmp |= 4;
        maxAxis = axisSelect[tmp];

        // arrangement
        // we're using the O(n) algorithm to reorder array into two part,
        // the smaller and bigger parts.
        division = completeBinaryTreeLeafDivide(rangeLen);
        division += r.first;
        r2 = r;
        while(TRUE) {
            pivotId = math::RndRange(r2.first, r2.second);
            //pivotId = r2.first; 
            pivotVal = leafIndex[pivotId];
            divideVal = ELEMENT_POSITION(pivotId)[maxAxis];
            leafIndex[pivotId] = leafIndex[r2.first];
            lid = r2.first;
            rid = r2.second - 1;
            while(lid < rid) {
                while(lid < rid && ELEMENT_POSITION(rid)[maxAxis] >= divideVal) --rid;
                leafIndex[lid] = leafIndex[rid];
                while(lid < rid && ELEMENT_POSITION(lid)[maxAxis] <= divideVal) ++lid;
                leafIndex[rid] = leafIndex[lid];
            }
            leafIndex[lid] = pivotVal;
            if(lid == division) break;
            else if(lid > division) r2.second = lid;
            else r2.first = lid + 1;
        };

        // the sub tree process
        r2.first = r.first;
        r2.second = division;
        callHeap.push(r2);
        r2.first = division;
        r2.second = r.second;
        callHeap.push(r2);
    }

#undef ELEMENT_POSITION
}

/* Divide leaf into two part (the left and right sub tree),
 * according to the arrangement of complete binary tree.
 * leafNum : the tree leaf number
 * return value: the left sub tree leaf number
 */
template <typename BVNode_t>
Int32 geom::BVTree_CBT<BVNode_t>::completeBinaryTreeLeafDivide( Int32 leafNum ) const
{
    // the full binary tree case
    if((leafNum & leafNum - 1) == 0) return leafNum / 2;
    // the other (common) case
    int childMax = 1;
    while((childMax << 1) < leafNum) childMax <<= 1;
    int childMin = childMax >> 1;
    int res = leafNum - childMin;
    if(res > childMax) res = childMax;
    return res;
}

LLIB_END
